#ifndef DRIVE_TYPE_H
#define DRIVE_TYPE_H

#include "Include.h"

namespace Team675
{

struct DriveInfo;

class DriveType
{	
public:
	typedef enum {ShiftAuto , ShiftHigh , ShiftLow, ShiftOff} ShiftSetting;
	virtual DriveInfo* Get() = 0;
};

struct DriveInfo
{
	bool  IsPID;
	float LeftSpeed;
	float RightSpeed;
	DriveType::ShiftSetting Shift;
	bool Pickup;
	bool RampArmDown;
	bool RampServoIn;
};

class ArcadeDrive  : public DriveType
{
	Joystick *m_stick;
	bool m_squareInputs;
	DriveType::ShiftSetting m_shiftSetting;
	bool m_balanceMode;
	Timer m_buttonTimer;
public:
	ArcadeDrive()
	{
		m_stick = new Joystick(DRIVE_STICK_2);
		m_squareInputs = true;
		m_shiftSetting = DriveType::ShiftLow;
		m_balanceMode = false;
		m_buttonTimer.Reset();
		m_buttonTimer.Start();
	}
	
	virtual DriveInfo* Get()
	{ 
		DriveInfo* drive = new DriveInfo;
		drive->IsPID = false;
		
//		if(m_stick->GetRawButton(3) && m_buttonTimer.HasPeriodPassed(0.8))
//		{
//			m_balanceMode = !m_balanceMode;
//			m_buttonTimer.Reset();
//			m_buttonTimer.Start();
//		}
		
		float t_moveValue = m_stick->GetY();
		float t_rotateValue = m_stick->GetX();
		
		if(m_squareInputs)
		{
			t_moveValue = sgn(t_moveValue) * t_moveValue * t_moveValue;
			t_rotateValue = sgn(t_rotateValue) * t_rotateValue * t_rotateValue;
		}
		
		if(m_balanceMode)
		{
			t_moveValue *= 0.3;
			t_rotateValue *= 0.1;
		}
		
		if(t_moveValue > 0.0)
		{
			if(t_rotateValue > 0.0)
			{
				drive->LeftSpeed = t_moveValue - t_rotateValue;
				drive->RightSpeed = max(t_moveValue, t_rotateValue);
			}
			else
			{
				drive->LeftSpeed = max(t_moveValue, -t_rotateValue);
				drive->RightSpeed = t_moveValue + t_rotateValue;
			}
		}
		else
		{
			if (t_rotateValue > 0.0)
			{
				drive->LeftSpeed = -max(-t_moveValue, t_rotateValue);
				drive->RightSpeed = t_moveValue + t_rotateValue;
			}
			else
			{
				drive->LeftSpeed = t_moveValue - t_rotateValue;
				drive->RightSpeed = -max(-t_moveValue, -t_rotateValue);
			}
		}
		if(m_stick->GetRawButton(4)) m_shiftSetting = DriveType::ShiftLow;
		if(m_stick->GetRawButton(5)) m_shiftSetting = DriveType::ShiftHigh;
		drive->Shift = m_shiftSetting;
		
		drive->Pickup = m_stick->GetRawButton(1);
		
//		if(m_stick->GetRawButton(1))
//		{
//			drive->RampArm = 1;
//		}
//		else if(m_stick->GetRawButton(2))
//		{
//			drive->RampArm = -1;
//		}
//		else
//		{
//			drive->RampArm = 0;
//		}
		return drive;
	}
};

class TankDrive  : public DriveType
{
	Joystick * m_leftStick;
	Joystick * m_rightStick;
	bool m_squareInputs;
	DriveType::ShiftSetting m_shiftSetting;
	bool m_balanceMode;
	Timer m_buttonTimer;
	bool m_rampDown;
	bool m_rampServoIn;
public:
	TankDrive()
	{
		m_leftStick = new Joystick(DRIVE_STICK_1);
		m_rightStick = new Joystick(DRIVE_STICK_2);
		m_squareInputs = true;
		m_balanceMode = false;
		m_shiftSetting = DriveType::ShiftLow;
		m_rampDown = false;
		m_rampServoIn = false;
	}
	
	virtual DriveInfo* Get()
	{ 
		DriveInfo* drive = new DriveInfo;
		
//		if(m_rightStick->GetRawButton(3) && m_buttonTimer.HasPeriodPassed(0.8))
//		{
//			m_balanceMode = !m_balanceMode;
//			m_buttonTimer.Reset();
//			m_buttonTimer.Start();
//		}
		
		drive->IsPID = false;
		if(m_squareInputs)
		{
			drive->LeftSpeed = sgn(m_leftStick->GetY()) * m_leftStick->GetY() * m_leftStick->GetY();
			drive->RightSpeed = sgn(m_rightStick->GetY()) * m_rightStick->GetY() * m_rightStick->GetY();
		}
		else
		{
			drive->LeftSpeed = m_leftStick->GetY();
			drive->RightSpeed = m_rightStick->GetY();
		}
		
		if(m_balanceMode)
		{
			drive->LeftSpeed *= 0.3;
			drive->RightSpeed *= 0.3;
		}
		
		if(m_rightStick->GetRawButton(4)) m_shiftSetting = DriveType::ShiftLow;
		if(m_rightStick->GetRawButton(5)) m_shiftSetting = DriveType::ShiftHigh;
		drive->Shift = m_shiftSetting;
		
		drive->Pickup = m_rightStick->GetRawButton(1);
		
		if(m_rightStick->GetRawButton(2)) m_rampDown = false;
		if(m_rightStick->GetRawButton(3)) m_rampDown = true;
		drive->RampArmDown = m_rampDown;
		
		if(m_rightStick->GetRawButton(8)) m_rampServoIn = false;
		if(m_rightStick->GetRawButton(9)) m_rampServoIn = true;
		drive->RampServoIn = m_rampServoIn;
		
		return drive;
	}
};

class AutonomousDrive : public DriveType
{
	DriveInfo* m_drive;
public:
	AutonomousDrive()
	{
		m_drive = new DriveInfo;
		m_drive->IsPID = false; 
		m_drive->Shift = DriveType::ShiftLow;
		m_drive->LeftSpeed = 0.0;
		m_drive->RightSpeed = 0.0;
		m_drive->Pickup = false;
		m_drive->RampArmDown = false;
		m_drive->RampServoIn = false;
	}
	
	void Set(float _left, float _right, bool _pickup)
	{
		m_drive->LeftSpeed = _left;
		m_drive->RightSpeed = _right;
		m_drive->Pickup = _pickup;
	}
	
	virtual DriveInfo* Get()
	{
		return m_drive;
	}
};

class KinectDrive : public DriveType
{
	KinectStick m_leftKinect, m_rightKinect;
public:
	KinectDrive() : 
		m_leftKinect(1),
		m_rightKinect(2)
	{
	}
	
	virtual DriveInfo* Get()
	{ 
		DriveInfo* drive = new DriveInfo;
		drive->IsPID = false; 
		drive->LeftSpeed = m_leftKinect.GetY();
		drive->RightSpeed = m_rightKinect.GetY();
		drive->Shift = DriveType::ShiftLow;
		drive->Pickup = m_leftKinect.GetRawButton(2);
		drive->RampArmDown = false;
		drive->RampServoIn = false;
		return drive;
	}
};

}
#endif
